//#include "stdafx.h"
#include "CAdvancedMeshCollision.h"
#include <float.h>
//#include <CColladaModularSkinnedMeshSceneNode.h>

#ifdef PFEXPORT
	#include <COpenGLTexture.h>
#endif

using namespace irr;
using namespace scene;
using namespace video;

// ----------------------------------------------------------------------------------------------
//! constructor
// ----------------------------------------------------------------------------------------------
CAdvancedMeshCollision::CAdvancedMeshCollision(IMesh* mesh, ISceneNode* node)
	: SceneNode(node)
	, Mesh(mesh)
	, MeshIsInWorldspace(false)
	, isStoringColors(true)
	, m_isHidden(false)
{
}

// ----------------------------------------------------------------------------------------------
//! constructor
// ----------------------------------------------------------------------------------------------
CAdvancedMeshCollision::CAdvancedMeshCollision(ISceneNode* node)
	: SceneNode(node)
	, Mesh(NULL)
	, MeshIsInWorldspace(false)
	, isStoringColors(true)
	, m_isHidden(false)
{
#ifdef _IRR_COMPILE_WITH_COLLADA_BINARY_LOADER_
	collada::CSkinnedMeshSceneNode* skinnedNode = NULL;
#endif

	switch(SceneNode->getType())
	{
#ifdef _IRR_COMPILE_WITH_COLLADA_BINARY_LOADER_
	case ESNT_COLLADA_SKIN_MESH:
	case ESNT_COLLADA_MODULAR_SKIN_MESH:
		skinnedNode = (collada::CSkinnedMeshSceneNode*)SceneNode;
		// support skinned mesh when they have a working buffer
		if (skinnedNode->isSkinningEnabled() || 
			((IColladaMesh*)skinnedNode->getMesh())->hasOutputBuffer())
		{
			// skinned mesh is in world space
			MeshIsInWorldspace = true;
		}
		Mesh = skinnedNode->getMesh();

		// Skinned mesh without output buffer are not supported...
		if (skinnedNode->isSkinningEnabled() && 
			!((IColladaMesh*)skinnedNode->getMesh())->hasOutputBuffer())
		{
			Mesh = NULL;
		}
		break;

	case ESNT_COLLADA_MESH:
#endif
	case ESNT_MESH:
	//case ESNT_BATCH_SCENE_NODE:
	//case ESNT_BATCH_GRID_SCENE_NODE:
		Mesh = ((IMeshSceneNode*)SceneNode)->getMesh();
		break;
		
	case ESNT_ANIMATED_MESH:
		IAnimatedMeshSceneNode* node = (IAnimatedMeshSceneNode*)SceneNode;
		Mesh = node->getMesh()->getMesh((s32)node->getFrameNr(),255, node->getStartFrame(), node->getEndFrame());
		break;
	}
}


// ----------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------
bool CAdvancedMeshCollision::isRayCollideMesh(const core::line3d<f32>& ray,
											core::vector3df& outCollisionPoint,
											core::triangle3df& outTriangle)
{
	u32 color = 0;
	isStoringColors = false;
	bool res = false;//isRayCollideMesh(ray, outCollisionPoint, outTriangle, color);//dai_yx
	isStoringColors = true;
	return res;
}


// ----------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------
//bool CAdvancedMeshCollision::isRayCollideMesh(const core::line3d<f32>& ray,
//											core::vector3df& outCollisionPoint,
//											core::triangle3df& outTriangle,
//											u32& color)
//{
//	if(Mesh == NULL || SceneNode == NULL)
//	{
//		return false;
//	}
//
//	core::line3d<f32> line(ray);
//	core::vector3df point;
//	bool found = false;
//	f32 nearest = FLT_MAX;
//
//	if(!SceneNode->getTransformedBoundingBox().intersectsWithLine(line))
//	{
//		return false;
//	}
//	
//	bool checkFacing = isStoringColors;
//
//	// Change the ray to the mesh's objectspace
//	if(!MeshIsInWorldspace)
//	{
//		core::matrix4 mat;
//		SceneNode->getAbsoluteTransformation().getInverse(mat);
//
//		mat.transformVect(line.start);
//		mat.transformVect(line.end);
//	}
//	
//	// Find min/max values for each axis to make faster checks
//	Min.set(core::min_(line.start.X, line.end.X),
//			core::min_(line.start.Y, line.end.Y),
//			core::min_(line.start.Z, line.end.Z));
//	Max.set(core::max_(line.start.X, line.end.X),
//			core::max_(line.start.Y, line.end.Y),
//			core::max_(line.start.Z, line.end.Z));
//	RayVect = line.getVector().normalize();
//	RayLength = line.getLengthSQ();
//
//	const u32 cnt = Mesh->getMeshBufferCount();
//
//	for (u32 i=0; i<cnt; ++i)
//	{
//		const IMeshBuffer* buf = Mesh->getMeshBuffer(i);
//		const u32 idxCnt = buf->getIndexCount();
//		const u16* const indices = buf->getIndices();
//		
//		if(buf->getVertexType() != EVT_COMPONENT_ARRAYS)
//		{
//			for (u32 j = 0; j < idxCnt; j += 3)
//			{
//				core::triangle3df triangle;
//				GetTriangle(triangle, buf, indices, j);
//
//				if(checkCollision(triangle, line, point, checkFacing))
//				{
//					const f32 tmp = point.getDistanceFromSQ(line.start);
//					const f32 tmp2 = point.getDistanceFromSQ(line.end);
//					//const f32 raylength = line.getLengthSQ();
//					if (tmp < RayLength && tmp2 < RayLength && tmp < nearest)
//					{	
//						nearest = tmp;
//						outTriangle = triangle;
//						outCollisionPoint = point;
//						found = true;
//					}
//				}
//			}
//		}
//		else
//		{
//			S3DVertexComponentArrays *pVertices = (S3DVertexComponentArrays *)buf->getVertices();
//			S3DVertexComponentArrays::SAccessorVector3df accessor;
//
//			if(pVertices->getPositionAccessor(accessor) == 0)
//			{
//				for (u32 j = 0; j < idxCnt; j += 3)
//				{
//					core::triangle3df triangle;
//					GetTriangle(triangle, accessor, indices, j);
//				
//					if(checkCollision(triangle,line,point, checkFacing))
//					{
//						const f32 tmp = point.getDistanceFromSQ(line.start);
//						const f32 tmp2 = point.getDistanceFromSQ(line.end);
//						//const f32 raylength = line.getLengthSQ();
//						if (tmp < RayLength && tmp2 < RayLength && tmp < nearest)
//						{	
//							nearest = tmp;
//							outTriangle = triangle;
//							outCollisionPoint = point;
//							found = true;
//							
//							if(isStoringColors)
//							{
//								color = GetColor(i, pVertices, indices, j, outCollisionPoint, outTriangle);
//							}
//						}
//					}
//				}
//			}
//			else
//			{
//				S3DVertexComponentArrays::SAccessorVector3ds accessor;
//
//				if(pVertices->getPositionAccessor(accessor) == 0)
//				{
//					for (u32 j = 0; j < idxCnt; j += 3)
//					{
//						core::triangle3df triangle;
//						GetTriangle(triangle, accessor, indices, j);
//						
//						if(checkCollision(triangle,line,point, checkFacing))
//						{
//							const f32 tmp = point.getDistanceFromSQ(line.start);
//							const f32 tmp2 = point.getDistanceFromSQ(line.end);
//							//const f32 raylength = line.getLengthSQ();
//							if (tmp < RayLength && tmp2 < RayLength && tmp < nearest)
//							{	
//								nearest = tmp;
//								outTriangle = triangle;
//								outCollisionPoint = point;
//								found = true;
//								
//								if(isStoringColors)
//								{
//									color = GetColor(i, pVertices, indices, j, outCollisionPoint, outTriangle);
//								}
//							}
//						}
//					}
//				}
//
//			}
//		}
//	}
//
//	if(found && !MeshIsInWorldspace)
//	{
//		core::matrix4 mat;
//		mat = SceneNode->getAbsoluteTransformation();
//
//		mat.transformVect(outCollisionPoint);
//		mat.transformVect(outTriangle.pointA);
//		mat.transformVect(outTriangle.pointB);
//		mat.transformVect(outTriangle.pointC);
//	}
//
//	return found;
//}

// ----------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------
bool CAdvancedMeshCollision::checkCollision( const core::triangle3df &triangle, const core::line3df &line, core::vector3df &outIntersection, bool checkFacing) const
{
	if(Min.X > triangle.pointA.X && Min.X > triangle.pointB.X && Min.X > triangle.pointC.X)
		return false;
	if(Max.X < triangle.pointA.X && Max.X < triangle.pointB.X && Max.X < triangle.pointC.X)
		return false;
	if(Min.Y > triangle.pointA.Y && Min.Y > triangle.pointB.Y && Min.Y > triangle.pointC.Y)
		return false;
	if(Max.Y < triangle.pointA.Y && Max.Y < triangle.pointB.Y && Max.Y < triangle.pointC.Y)
		return false;
	if(Min.Z > triangle.pointA.Z && Min.Z > triangle.pointB.Z && Min.Z > triangle.pointC.Z)
		return false;
	if(Max.Z < triangle.pointA.Z && Max.Z < triangle.pointB.Z && Max.Z < triangle.pointC.Z)
		return false;

	if(checkFacing && !triangle.isFrontFacing(RayVect))
	{
		return false;
	}

	if (triangle.getIntersectionWithLine(line.start, RayVect, outIntersection))
	{
		const f32 tmp = outIntersection.getDistanceFromSQ(line.start);
		const f32 tmp2 = outIntersection.getDistanceFromSQ(line.end);

		if (tmp < RayLength && tmp2 < RayLength)
		{
			return true;
		}
	}
	return false;
}

// ----------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------
//void CAdvancedMeshCollision::GetTriangles(core::array<core::triangle3df>& triangles)
//{
//	if(Mesh == NULL || SceneNode == NULL)
//	{
//		return;
//	}
//
//	const u32 cnt = Mesh->getMeshBufferCount();
//
//	for (u32 i=0; i<cnt; ++i)
//	{
//		const IMeshBuffer* buf = Mesh->getMeshBuffer(i);
//		const u32 idxCnt = buf->getIndexCount();
//		const u16* const indices = buf->getIndices();
//		
//		if(buf->getVertexType() != EVT_COMPONENT_ARRAYS)
//		{
//			for (u32 j = 0; j < idxCnt; j += 3)
//			{
//				core::triangle3df triangle;
//				GetTriangle(triangle, buf, indices, j);
//				TransformTriangle(triangle);
//				triangles.push_back(triangle);
//			}
//		}
//		else
//		{
//			S3DVertexComponentArrays *pVertices = (S3DVertexComponentArrays *)buf->getVertices();
//			S3DVertexComponentArrays::SAccessorVector3df accessor;
//
//			if(pVertices->getPositionAccessor(accessor) == 0)
//			{
//				for (u32 j = 0; j < idxCnt; j += 3)
//				{
//					core::triangle3df triangle;
//					GetTriangle(triangle, accessor, indices, j);
//					TransformTriangle(triangle);
//					triangles.push_back(triangle);
//				}
//			}
//			else
//			{
//				S3DVertexComponentArrays::SAccessorVector3ds accessor;
//
//				if(pVertices->getPositionAccessor(accessor) == 0)
//				{
//					core::triangle3df triangle;
//					
//					for (u32 j = 0; j < idxCnt; j += 3)
//					{
//						core::triangle3df triangle;
//						GetTriangle(triangle, accessor, indices, j);
//						TransformTriangle(triangle);
//						triangles.push_back(triangle);
//					}
//				}
//			}
//		}
//	}
//}

// ----------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------
void CAdvancedMeshCollision::GetTriangle(core::triangle3df& triangle, const IMeshBuffer* buf, const u16* const indices, s32 start)
{
	#ifndef _IRR_USE_RIGHT_HAND_CONVENTION_
		triangle.pointA = buf->getPosition(indices[start + 0]);
		triangle.pointB = buf->getPosition(indices[start + 1]);
		triangle.pointC = buf->getPosition(indices[start + 2]);
	#else
		triangle.pointA = buf->getPosition(indices[start + 2]);
		triangle.pointB = buf->getPosition(indices[start + 1]);
		triangle.pointC = buf->getPosition(indices[start + 0]);
	#endif //_IRR_USE_RIGHT_HAND_CONVENTION_
}

// ----------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------
//void CAdvancedMeshCollision::GetTriangle(core::triangle3df& triangle, S3DVertexComponentArrays::SAccessorVector3ds& accessor, const u16* const indices, s32 start)
//{
//	#ifndef _IRR_USE_RIGHT_HAND_CONVENTION_
//		accessor.get(indices[start + 0], triangle.pointA);
//		accessor.get(indices[start + 1], triangle.pointB);
//		accessor.get(indices[start + 2], triangle.pointC);
//	#else
//		accessor.get(indices[start + 2], triangle.pointA);
//		accessor.get(indices[start + 1], triangle.pointB);
//		accessor.get(indices[start + 0], triangle.pointC);
//	#endif //_IRR_USE_RIGHT_HAND_CONVENTION_
//}


// ----------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------
//void CAdvancedMeshCollision::GetTriangle(core::triangle3df& triangle, S3DVertexComponentArrays::SAccessorVector3df& accessor, const u16* const indices, s32 start)
//{
//	#ifndef _IRR_USE_RIGHT_HAND_CONVENTION_
//		triangle.pointA = accessor[indices[start + 0]];
//		triangle.pointB = accessor[indices[start + 1]];
//		triangle.pointC = accessor[indices[start + 2]];
//	#else
//		triangle.pointA = accessor[indices[start + 2]];
//		triangle.pointB = accessor[indices[start + 1]];
//		triangle.pointC = accessor[indices[start + 0]];
//	#endif //_IRR_USE_RIGHT_HAND_CONVENTION_
//}

// ----------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------
void CAdvancedMeshCollision::TransformTriangle(core::triangle3df& triangle)
{
	if(!MeshIsInWorldspace)
	{
		core::matrix4 mat;
		mat = SceneNode->getAbsoluteTransformation();

		mat.transformVect(triangle.pointA);
		mat.transformVect(triangle.pointB);
		mat.transformVect(triangle.pointC);
	}
}


// --------------------------------------------------------------------------
/// @brief Gets interpolated color value according to the position of the
/// collision point inside the triangle
// --------------------------------------------------------------------------
//u32 CAdvancedMeshCollision::GetInterpolatedColor(const irr::core::vector3df& collisionPoint, const irr::core::triangle3df& collisionTriangle,
//							u32 color1, u32 color2, u32 color3)
//{
//	f32 dist1 = collisionPoint.getDistanceFromSQ(collisionTriangle.pointA);
//	f32 dist2 = collisionPoint.getDistanceFromSQ(collisionTriangle.pointB);
//	f32 dist3 = collisionPoint.getDistanceFromSQ(collisionTriangle.pointC);
//	
//	// TODO: check if this calculation is correct
//	irr::video::SColor c1(color1);
//	irr::video::SColor c2(color2);
//	irr::video::SColor c3(color3);
//	c2 = c1.getInterpolated(c2, dist1 / (dist1 + dist2));
//	c3 = c1.getInterpolated(c3, dist1 / (dist1 + dist3));
//	c1 = c2.getInterpolated(c3, 0.5f);
//	
//	return c1.toARGB8();
//}

// --------------------------------------------------------------------------
// --------------------------------------------------------------------------
//u32 CAdvancedMeshCollision::GetColor(s32 bufindex, S3DVertexComponentArrays *pVertices, 
//									const u16* const indices, s32 start,
//									const core::vector3df& outCollisionPoint,
//									const core::triangle3df& outTriangle)
//{
//	u32 color = 0;
//	
//#ifdef PFEXPORT
//	bool getTexture = true;
//	s32 unit = 1;
//	
//	// on lightmaps get lightmap texture
//	video::SMaterial& mat = SceneNode->getMaterial(bufindex);
//	s32 type = mat.getMaterialType();
//	if(type != EMT_LIGHTMAP
//		&& type != EMT_LIGHTMAP_LIGHTING
//		&& type != EMT_LIGHTMAP_ADD 
//		&& type != EMT_LIGHTMAP_M2 
//		&& type != EMT_LIGHTMAP_M4
//	)
//	{
//		getTexture = false;
//	}
//
//	
//	ITexture* texture = 0;
//	IImage* image = 0;
//	if(getTexture)
//	{
//		texture = mat.getTexture(unit);
//
//		if(!texture)
//		{
//			getTexture = false;
//		}
//		else
//		{
//			COpenGLTexture* tex = static_cast<COpenGLTexture*>(texture);
//			image = tex->getImage();
//			if(!image)
//			{
//				getTexture = false;
//			}
//		}
//	}
//	
//	if(getTexture)
//	{
//		// get color value according to lightmap coordinates
//		
//		// get uv coordinates
//		core::vector2df p, p1, p2, p3;
//		S3DVertexComponentArrays::SAccessorVector2ds accessor;
//		if(pVertices->getTexCoordAccessor(unit, accessor) == 0)
//		{
//			#ifndef _IRR_USE_RIGHT_HAND_CONVENTION_
//				accessor.get(indices[start + 0], p1);
//				accessor.get(indices[start + 1], p2);
//				accessor.get(indices[start + 2], p3);
//			#else
//				accessor.get(indices[start + 2], p1);
//				accessor.get(indices[start + 1], p2);
//				accessor.get(indices[start + 0], p3);
//			#endif //_IRR_USE_RIGHT_HAND_CONVENTION_
//		}
//		else
//		{
//			S3DVertexComponentArrays::SAccessorVector2df accessor;
//			if(pVertices->getTexCoordAccessor(unit, accessor) == 0)
//			{
//				#ifndef _IRR_USE_RIGHT_HAND_CONVENTION_
//					p1 = accessor[indices[start + 0]];
//					p2 = accessor[indices[start + 1]];
//					p3 = accessor[indices[start + 2]];
//				#else
//					p1 = accessor[indices[start + 2]];
//					p2 = accessor[indices[start + 1]];
//					p3 = accessor[indices[start + 0]];
//				#endif //_IRR_USE_RIGHT_HAND_CONVENTION_
//			}
//			else
//			{
//				// no texture coordinates coord
//				return 0;
//			}
//		}
//
//		core::dimension2di size = texture->getSize();
//		
//		//find barycentric coord
//		core::vector3df BA = outTriangle.pointB - outTriangle.pointA;
//		core::vector3df AC = outTriangle.pointA - outTriangle.pointC;
//		f32 inv = (BA.X * -AC.Y - BA.Y * -AC.X);
//
//		if ( inv == 0 ) 
//		{
//			//DBG("WARNING - check mesh \n");
//			return 0;
//		}
//
//		f32 div = 1.0f / inv;				
//		f32 u = ((outCollisionPoint.X - outTriangle.pointB.X) * (outTriangle.pointB.Y - outTriangle.pointC.Y) - (outTriangle.pointB.X - outTriangle.pointC.X) * (outCollisionPoint.Y - outTriangle.pointB.Y)) * div;
//		f32 v = ((outCollisionPoint.X - outTriangle.pointC.X) * (outTriangle.pointC.Y - outTriangle.pointA.Y) - (outTriangle.pointC.X - outTriangle.pointA.X) * (outCollisionPoint.Y - outTriangle.pointC.Y)) * div;
//		f32 w = 1 - v - u;
//
//		// apply barycentric coordinates to get uv values of the coordinate
//		p.X = p1.X + (p2 - p1).X * v - (p1 - p3).X * w;
//		p.Y = p1.Y + (p2 - p1).Y * v - (p1 - p3).Y * w;
//
//		// flip y coordinate because pvr textures are flipped
//		p.Y = 1.0f - p.Y;
//
//
//		if(texture->getWrapS() == ETC_REPEAT)
//		{
//			while(p.X < 0.0f) p.X += 1.0f;
//			while(p.X > 1.0f) p.X -= 1.0f;
//		}
//		if(texture->getWrapT() == ETC_REPEAT)
//		{
//			while(p.Y < 0.0f) p.Y += 1.0f;
//			while(p.Y > 1.0f) p.Y -= 1.0f;
//		}
//
//		s32 su = static_cast<s32>(size.Width * p.X);
//		s32 sv = static_cast<s32>(size.Height * p.Y);
//		
//		SColor c = image->getPixel(su, sv);
//		color = c.toARGB8();
//	}
//	else
//	{
//		// get color value according to vertex color
//		S3DVertexComponentArrays::SAccessorColor accessor2;
//		if(pVertices->getColor0Accessor(accessor2) == 0)
//		{
//			#ifndef _IRR_USE_RIGHT_HAND_CONVENTION_
//				u32 color1 = accessor2[indices[start + 0]].toARGB8();
//				u32 color2 = accessor2[indices[start + 1]].toARGB8();
//				u32 color3 = accessor2[indices[start + 2]].toARGB8();
//			#else
//				u32 color1 = accessor2[indices[start + 2]].toARGB8();
//				u32 color2 = accessor2[indices[start + 1]].toARGB8();
//				u32 color3 = accessor2[indices[start + 0]].toARGB8();
//			#endif //_IRR_USE_RIGHT_HAND_CONVENTION_
//			
//			color = GetInterpolatedColor(outCollisionPoint, outTriangle, color1, color2, color3);
//		}
//	}
//#endif
//	return color;
//}